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ABSTRACT 


This thesis presents the dynamic modeling and its application to the 
multi-link robot manipulators with joint flexibility. The twist angles of 
springs are utilized to model the joint flexibility. A direct dynamic model is 
derived by the Lagrangian method and the Newton-Euler algorithm is 
used for deriving the inverse dynamics. A motion controller is designed 
by the computed torque method based on the inverse dynamics. A 
recursive estimator is included in the control design to estimate the 


required feedback signals which are not directly measurable. 
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I. INTRODUCTION 


The problem of joint flexibility has been recognized in most industrial 
robot designs. Joint flexibility typically results from the torsional flexibility 
of the gear mechanism and drive shafts. The effect of joint flexibility is 
negligible for low-speed and light payload operations and the rigid-body 
controllers are able to successfully control the system. The rigid-body 
controllers refer to controllers that are designed based on a rigid-body 
dynamic model. As the speed and payload increase, the inertia forces 
and gravity forces increase such that the flexibility effect becomes 
significant. The flexibility may not be tolerated by the rigid-body controller 
and may cause system instability. The improvement of the dynamic 
behavior requires that the joint flexibility effects be incorporated into the 
controller design for industrial robots. A dynamic model of industrial 
robots with joint flexibility is therefore needed. 

The Lagrangian method was applied to rigid-body manipulators to 
obtain the direct dynamic model where the kinematics was described by a 
set of homogeneous transformation matrices [Ref. 1: pp. 158-172]. The 
recursive Newton-Euler method was used for solution of the inverse 
dynamic response to compute the required torques for rigid-body 
manipulators [Ref. 2: pp. 168-172]. Several nonlinear control techniques, 
namely feedback linearization, integral manifold and composite control 
[Ref. 3] were applied to design controllers for a single-link manipulator 
with joint elasticity. A position control was developed for a two-degree- 


of-freedom manipulator where the effects of drive-train compliance and 


actuator dynamics were included [Ref. 4]. The control law was derived 
from the concepts of “computed torque" where the inverse dynamics 
were utilized to determine the required torques as the functions of 
position, velocity, acceleration, jerk and jerk rate errors. With the same 
control scheme, the computer simulation and dynamic analysis of a single- 
joint robot was conducted [Ref. 5]. The third joint of the PUMA 560 
robot was selected as an example and a comparison between the 
performances of rigid-body controller and flexible-body controller was 
performed. 

In this thesis, both the Lagrangian dynamics [Ref. 1] and the Newton- 
Euler algorithm [Ref. 2] will be applied to multi-link robot manipulators 
with joint flexibility. The former is used to derive the direct dynamics for 
the plant, computing the joint motion provided that the torque inputs are 
given. The latter is used to develop the inverse dynamics which returns 
the required torques assuming the joint motion is known. This thesis also 
uses the method of computed torque [Ref. 4] to design a control law for 
multi-link robot manipulators with joint flexibility. This control approach 
presents a method of decoupling the arm motion when drive-train 
compliance and actuator dynamics are considered. It is assumed that the 
angular positions and velocities of both links and motors are measurable 
from shaft encoders and tachometers. Since the second and third 
derivatives of link positions (i,e., accelerations and jerks) are required in 
the control design and not directly measurable, an estimator is designed in 
a recursive manner to obtain the second and third derivatives of link 


positions. 


To present the frame work of this thesis, Figure 1 shows an error- 
driven feedback control system which includes the inverse dynamics 
(controller), direct dynamics (plant) and the estimator. In this figure, the 
system is designed to follow a reference input and the system error is 
generated by the difference between the desired reference input and the 
feedback. The Newton-Euler method is used to design the controller 
since its recursive formulation allows for the efficient computation of the 
inverse dynamics and therefore possibly for on-line control. The 
Lagrangian method derives the direct dynamics and models the plant for 
computer simulation because of its systematic nature which is convenient 
for programming. 

The objective of this thesis is to formulate the direct and inverse 
dynamics of multi-link robot manipulators with joint flexibility. In addition, 
a computed-torque control is designed to show an application of the 
inverse dynamics. Notations and preliminaries will be presented in 
Chapter II. The direct dynamics and the inverse dynamics will be 
discussed in Chapter IIJ and Chapter IV, respectively. Finally, the 
controller and estimator design procedures will be developed in Chapter 


V. 
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Il. NOTATION AND PRELIMINARIES 


This section defines the notations used in the thesis and briefly 


discusses some of the basic fundamentals which will be used to develop 
the model. 


A. NOTATION AND PRELIMINARIES FOR THE DIRECT 
DYNAMICS 


a; 


q; 


q ni 
G, 


A. 


é 


: one of the four Hartenberg-Denavit parameters representing the 


distance between two adjacent frames i-1 and : in x direction. 


: one of the four Hartenberg-Denavit parameters representing the 


rotation angle about the x axis. 


; one of the four Hartenberg-Denavit parameters representing the 


distance between links in z direction. 


: one of the four Hartenberg-Denavit parameters representing the 


rotation angle of links about the z axis. 


: joint variable of link 7. For prismatic joints 7, 4; is d,, for revolute 


joint 7, G; is 6,. 


: the rotation angle of rotor z about the z axis. 


: gear ratio, 1.e., 


rotation angle of z- th motor shaft 


G.= (2.1) 


rotation angle of 7 - th link 


: the Hartenberg-Denavit matrix for link 1, Le., 


] 0 0 @) 
acosq. cosq. -sing.cosa@. sing.sina. 
Al =a ia : e * mie (> 
asing, sing, cosq,cos@, -cosq,sina. 


d. 0 sing. cosa. 


8 


A.,, : homogeneous (4X4) coordinate transformation matrix of motor 


] 0 0 0 
0 -sing,, O 
ie., A_-|. <a (2.3) 
O sing,, cosq,, O 
| 0 0 0 l 
where 
Q ni = G,q, }, (2.4) 


W, : homogeneous (4X4) coordinate transformation matrix from the /- 
th coordinate frame to the base coordinate frame. 

W; : homogeneous (4X4) coordinate transformation matrix from the 
i-th coordinate frame to the j-th coordinate frame. 

m, : mass of the i-th link. 

m_, : mass of the i-th motor. 


r, : local position vector of the center of mass of link i decomposed in 


the body-fixed coordinate. 


r,; absolute position vector of the center of mass of link ; 
decomposed in the base coordinate. 

r,,,; absolute position vector of the center of mass of motor 1 
decomposed in the base coordinate. 

r’.: : local position vector of center of mass of motor ; decomposed 


in the motor body- fixed coordinate. 
J__: the pseudo inertia matrix of the i-th motor 





einer) dia 
O O 0 
I. 
O MiZZ O 
é I 
“10 0 mies 0 2) 





where I _. is the moment of inertia of motor : in the local z axis. 
Note: The elements in the first row and the first column are zero 
Since the mass m_. of the motor / is considered to be included in 
the link 1. 

J, : the pseudo inertia matrix of the :-th link, where 


J, =Jriry dm 


= ee lee 
7), x; z is I, 
Z 
= ele 
f iy sa ibe (2.6) 
Mm; y; 2 
A eee 
M,Z; i I; z 
: 2 


where m,x,, m.y., m,Z, are the first moments of link i, I,,, 1,,, I, 


ie yy 122 


are the moments of the inertia of link 7 in the principal axes and 
I. 1, ti, are the products of inertia of link 1. 
B. NOTATION AND PRELIMINARIES FOR INVERSE 


DYNAMICS 
Ore G,q, — Gai ; 


i 


: absolute angular velocity of link 7. 


™. 


=, 


: absolute angular acceleration of link 7. 


: change rate of @.. 


: change rate of @.. 


: absolute angular velocity of link 2 ina matrix form. 


™, 


So) eo ec So 


: local relative angular velocity of link i decomposed in coordinate 


i-l,ie,(0 0 q,]' 


= 


©. 


;, : local relative angular acceleration of link i decomposed in 


coordinate i-1,ie.,{0 0 d.] 


@,, : local relative angular jerk of link : decomposed in coordinate i-1, 
ie.,[0 0 @,]’ 

a, : linear acceleration of link i_ 

@. : absolute angular velocity of motor i decomposed in coordinate !- 
. 

@,,; : change rate of @.,. 

a., : linear acceleration of link j at the center of mass. 

F: : resultant force exerted on link i at the center of mass. 

f.: force exerted on link 7 by link: —1, 

N;: resultant moment exerted on link 7 about the center of mass. 

n, : moment exerted on link 7 by link 1-1. 

n_, : moment exerted on motor 1 decomposed in coordinate 1. 

T. : torque generated from the electromagnetic field of motor in the 
local z axis. 

R/ : rotational transformation (3 x3) matrix from the j-th coordinate 
frame to the 1-th coordinate. 

K..: control gain. 

k. : spring stiffness. 

k : unit vector in the local z axis. 

I 


. : inertia tensor of link 1, 1.e., 


Is 1xy Ii. 
J = i ly i C27) 
ee 1yz i 


]_, : moments of inertia of motor 1, 1.e., 


to 0 
I,=| 0 I,,, 0 (2.8) 
O09 The 


II. DIRECT DYNAMICS 


The Lagrangian dynamics approach is used to derive direct dynamics 
for the equation of motion of the plant in terms of the generalized 


coordinates. The Lagrangian dynamic equations are given as : 


d/ O(KE)) O(KE) O(PE)_, ,.. 
“( ag. | aé 3 ae =P. Cel y 2 on n) (3.1) 


where ¢, is the generalized coordinate, n is the number of degrees of 
freedom and P. denotes the generalized force. 

This thesis considers a plant of a six-link manipulator with revolute 
joints actuated by DC-motors. Each motor is connected through a 
transmission shaft (Figure 2) to a rigid link and a spring with a constant 


stiffness 1s modeled to be a major source of joint flexibility. The twist 


angle of the spring 6, is defined as 6. =G,q,-—q,, where G, is the gear 
ratio. Since additional degrees of freedom are introduced by the twisted 


spring , the generalized coordinates are defined as: 


= Gs. a, ++, 6,,55, «+ 5] 


The generalized force P, is obtained from the concept of virtual work. 


Since 


6W=) 76 Clee =) 1,(G,6 q; —00,) 


1] 
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FIGURE 2. Single-Link Manipulator With Joint Flexibility 


PZ 


therefore, T,G, is the generalized force corresponding to the generalized 
coordinate q,, and —T, is the generalized force corresponding to the 
generalized coordinate 6.. 

The total kinetic energy of the system consists of the individual kinetic 
energies of the links and motors and the total potential energy is 
composed of the strain energy due to joint elasticity and the potential 
energy due to gravity. The kinetic energy KE and the potential energy PE 
are derived as follows: Given a point specified by a local position vector 
r, (Figure 3), its position and velocity with respect to base coordinates are 
given as 

P= ae Gy) 
and 
r= Wr ee) 


The kinetic energy of link 7 is 
(KE), = str) hiTdm (3.4) 


For six-link robot manipulators, the total kinetic energy becomes 
6 
Ua) = SU CIE 
t=] 


= yet] rr"dm (3.5) 


i=] 
Substituting Equation (3.3) into Equation (3.5) gives 
> | } boa! } 
(KE),, = > 5 Wi ( [rin/"dm)W, | (3.6) 


8 


yj 
z 


coordinate 1 


x. 





Yo 


Figure 3. Position Vector of The Center of Mass of Link 7 


The integral in Equation (3.6) is known as the pseudo inertia matrix J, 
and was given in section II. Equation (3.6) thus becomes 


(KE),, = you WA, W,| (3.7) 


i=] 


Similarly, the kinetic energy of the motors is 


=Yaufie," (3.8) 
inl 2 
where 
VAT OD), 
The time derivative of r_, is obtained as 
=W,_A_ri,+W,_A ri, (3.10) 
and 
I 0 0 0 
O cosg., -sing.. O 
Aw = 0 aig iim 0 — 
| O 0 0 1 | 


Substituting Equation (3.10) into Equation (3.8) gives 


(KE), = .=-t[(W, Ari, + W,, Ari] 


mi mi 
i=] 


(Wo Aur + Wo Agri) [dm 


= fof WA (rear) 


mimi 


+W,_A,,( rir ;dm )A?, We 


+W_A =| rir dm JA, We, 


mm mi 


i-] 


+W,_A,, (fri “dm)Aq,W,, | (3.12) 
Since 
uf WA, ([rarcdm)ALWe, |= tr] WA, (frircdm)ALW,] (3.13) 


Equation (3.12) becomes 


6 
(KE)_ =Y5u[WoA., JAW 
+2W_A J ATW" 


+W_A_J m2, | (3.14) 


i-] 


where 


J = rir’ dm (3.15) 


and the total kinetic energy of the system can be found as 
(KES (Ke)e (Kee (Sia 


The potential energies of links and motors are obtained as 

6 
t=] 
= ;T T 

=-) mr, W, g (Salers 
i=) 

and 
6 
28) pe 2 mr. g 
= -Em, (WA...) 2 


=-S moray Wg (3.18) 


where 





The center of mass of link 7 is described by a local vector r:, and the 
center of mass of motor / is described by a local vector r__,. Their position 
transformations from local coordinate to base coordinate are defined as 


r, = Wir, and r_,, = W,_,A_,r_,, respectively. 


mi cmi ? 


The potential energy due to strain is 


(PE). = 5K 6 (3.19) 


Therefore, the total potential energy of the system is obtained by 
combining Equations (3.17), (3.18) and (3.19) : 


6 Gr T 6 -T T U ] 
(PE) =-> m,r’, W, g- >})m_r! ,A_,W,_.g + 5 ask.0, (3.20) 
i=] t=] s=1 


mi cm 


Since generalized coordinate q, is for large motion and generalized 
coordinate 0. is for small motion, two sets of equation of motion will be 
obtained according to Equation (3.1). For the large motion, the general 


form of dynamic equation is described as 


> ir(W,J,W, )+t(X JAW.) + ¥ tr(X,J PANN: | 
i=j i=j+l 
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6 
= aie W,g-m, ri A, WwW. i8— Sm TonifS i- W,, 2= Ga 
=J 


i=j+l 


(3.215 
where 
X,=W,A, (3.22) 
X,=W,,A_.+W,,A,, (3.23) 
X =W_A, +2W,,AW, A, (3.24) 
The dynamic equation for the small motion is described as 
-r(X JA, ,W,,)+m nr Al, W.4g+k,6, =-T, (3.25) 


The second time derivative of the transformation matrix W. can be 
derived as 


W, => Wd +>>Ww mead. (3.26) 


k=] m=! 


where >> W.,.4 .q,, 18 defined as the residual term that includes Coriolis 


k=] m=] 
and centripetal accelerations. A recursive form (see Appendix A) is used 
to compute the residual term to avoid tedious computation. The detailed 
procedure of direct dynamic equations is derived in Appendix B. 
For the special case of a rigid body plant, the stiffness of the spring 
approaches infinity. Therefore, the elastic displacement is equal to zero, 


and Equation (3.21) for the rigid body case reduces to : 


ON 


i 


ir(W,J,W, )- Sim,r" Wg =T, (3.299 


J i=j 


-, 
i] 
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In this case, the Equation (3.25) presenting the small motion can be 
ignored directly. 


IV. INVERSE DYNAMICS 


The inverse dynamics is to predict either the required driving force or 
torque for a mechanical system of which the motion (i.e., the joint 
positions, velocities, accelerations and jerks in this thesis) is known. The 
inverse dynamics of robot manipulators with joint flexibility is developed in 
this section by the Newton-Euler method. The controller and the 
estimator are designed based on the inverse dynamics for that is 
computationally efficient and has potential for on-line control. 

Figure 4(a) presents a free body diagram of the link 7 where f, and n. 


are force and torque exerted by link 7-1 decomposed in coordinate 7 and 
f 


i+] 


and n,,, are force and torque exerted by link i+1 decomposed in 
coordinate 1 +1. This system of forces and moments is equivalent to that 
on Figure 4(b) where F and N. are the resultant force and torque acting 
on the center of mass. Therefore, the force and moment balance 


equations are obtained as 


F=f Ree (a5 
N, =n, -R"n,,, -Sl, xf, -Cl, xF (4.2) 
The Newton-Euler equations can then be written as 
F=m.a, (4,3) 
N, =1Lo, +Q,(1,@,) (4.4) 


By taking balance of moments in the local z direction on the free body 
diagram of the motor / (the bottom figure of Figure 5), the torque equation 


can be expressed as 
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(a) 


-Ri'¢ 


i i+] 


Link 7 


tutes plawe 


Figure 4(a). Free-Body Diagram of Link ; 


2] 


i+] 
-Roon 






coord. i 


i+] 





(b) 


Figure 4(b). Resultant Force and Torque of Link; 


Ze 


T, =(Ri_jn,)-k+(I,,,, +Q,_,1,,@,, )-k (4.5) 


i i-l 3 


where 


@O..=0,,+4 0 (4.6) 
and 


0 

@O..=0.,+4 0 (4.7) 
From Equations (4.5), (4.6) and (4.7), the required torque T. can be 
obtained if the kinematics of the manipulator is given. The recursive 
inverse dynamics of rigid-body systems has been available [Ref. 6] where 
the gravity has been incorporated into the kinematics. For flexible-joint 
systems, the motor variables are not identical to the link variables and 4,,, 
and q,,; will be related to link variables in this research. 

Figure 5 shows three free-body diagrams, 1.e., free-body diagram of 
link i, the i-th spring and motor 1, which illustrate the relations among the 
moment n,, the spring torque k,6, and the torque T, produced by the 
electromagnetic field of the motor. Since only the moments in local z axis 
are concerned, the moments acting on the both sides of the spring along z 
axis are equivalent in magnitude and opposite in directions. In other 
words, the component of the moment n. in the local z axis acting on link 1, 
ne; (Ri_n,)-k, is equal to that acting on the spring in the local z axis. 


This relation can be given as 


ye 





k,3, ——>A\\\\ = (Rin) -k 


k,6, =k,(G.q, -4,) = (Ri_,n,) “Kk 


(I,,Q.n; ue CL Le) j k 





Figure 5. Free-Body diagram of a link, a spring and a motor 
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Rf 


i+] 


k,6, =(Ri_,n,)-k (4.8) 
Or 


a 
0, = (Rian), k (4.9) 


i 


As we defined 6. in section II, 4, can be described as 

bere Gq; a 0, (4.10) 
By substituting Equation (4.9) into Equation (4.10), the angular position of 
motor J, 1.€., dai, can be expressed as a function of generalized coordinate 
q;: 


i-l 


q_, =G.q, -—(R n )-k 


The above equation is equivalent to the following equation where the 


vectors are decomposed in the base coordinates. 
ojos - 
Qni = Gig, ~ 7— (Ran, )-(Ro'k) (4.14) 


As referring to Equation (4.7), the first and second time derivatives of 


angular position of motor i are required to compute the torque T;. By 


applying R? =R © es q.,, and 4,, can be obtained as 


q., =G.q, - —[Ri (Qn, +n,)-(Ri'k)+(Rjn,)- (RiQ,_,k)] (4.15) 


i 


i. = 6.4, - {Ri (O.2.n, +2.n, +20,8, +11,)-(Ro'k) 
+2R)(Q,n, +n,)-(Ri'Q, .k) +(Rin,)-[Ro(Q,.k + 2,2, .k)]| 


i-l 


Zs) 


where 


(4.16) 


n, =N,+Rj"n,,, +S], xf,+Cl, xF (4.17) 
n, =Q,N,+N,+Ri"(Q,,n,,, +n,,,) +2, (SI, xf,) +S, xf, 

+Q,(Cl, xXF)+Cl,xF -Qyn, (4.18) 
fi, = Q,Q,N, +20,N, +Q,N, +N, +Ri"(Q,,,Q,,.n,,, 

+20... +Q,.n,., +ii,, +Q,0,(S1, xf.) 

+ 20, (SI, x f,)+,(SI, x f,) + SI, xf, +Q,Q;(Cl, x F) 

+ 20,(Cl, x F)+2,(Cl, xF,)+Cl, x F -Q.Q.n, 

—2Q.n, - Qn. (4.19) 
N, =1,@, + Q,(1,@, ) (4.20) 
N, = 1,6, +Q1@, +Q,1,, (412m) 
N, =1,6, +Q,1,@, + 2Q,1,@, + Q1,0. (4.22) 
wo, =Ri"(o,, + @,,) (4.23) 
@, =Ri"(Q,,@,, + @,_, + @,,) (4.24) 
0, =RI"(Q,,Q,,@,,+Q,_,0,, +2Q,,@,, +Q,,@,,+ @,, 

+6), ,)- 2,0, (4.25) 

@, = R'*(Q, Q, Q,,@,, +Q,,Q, ,.@,, +3Q,,2,,@,, 

+20, 0, ,0,,+2Q,_,6,, +3Q,,0,, +Q,,Q,,@,, 

+Q,.0,, +3Q,,0,,+0,,0,,+ @,, + é,,) 


-(Q,0,0, + 22,0, + 2,0, ) (4.26) 


26 


The intermediate variables f,, F, and their rates of change are described 
in Appendix C. 


Since 


o,, =40 (4.27) 
qi 


differentiating Equation (4.27) with respect to time gives 


0 0 0 
@,,=10$6,,=40}8, =4 0 (4.28) 
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IV 


i i q; 
b ] b ] 


Substituting Equations (4.27-28) into Equation (4.5) yields a fourth- 
order ordinary differential equation. The inverse dynamics can be simply 
describes as 

Ti=35q) 1. (4.29) 
where f, is a nonlinear function of arm position, velocity, acceleration and 
jerk, the term J; is a function of the moments of inertia of motors and links 


and spring constants. T, is the required torque. 


4) 


IV. CONTROLLER AND ESTIMATOR DESIGN 


The purpose of designing an controller for the manipulator is to drive 
the tracking error to zero asymptotically. In this section, a method of 
computed torque [Ref. 4] is utilized as a control scheme to determine the 
required torques. 

Define the error as 

E=qy-4; (Say 
where q, is the desired angular position of link i and q, is the measured 
angular position of link 7. The error dynamics for the fourth-order system 
(Equation (4.29)) is described as: 

é’ +K,€+K,,é€+K,€+K,,€ =0 (5.2 
where the K's are constants representing the feedback gains. By 
substituting q*” in Equation (4.29) to the error Equation (5.2), the 
computed torque is then obtained as 


T= 32 queen (5.3) 


i ica] 


where J° 


ical 


is a calculated value of J’ and f,,, is the calculated value of f.. 


dia is the "calculated jerk rate" given by 


qe, =O) +ks(do— 4) +k(Gn - 4.) + kalo -4:)+ko(Gn-4,) 4) 


where q,,q,,q, and q, are measured variables. J’and f,,, can be 


ical 
computed from Equation (4.29) based on the complete knowledge of the 
plant dynamics and availability of measurements. 

ITAE performance criteria [Ref. 7] for a fourth-order equation is used 


for the selection of the k values. The values are 
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k,, =2.10, 


k,, =3.4@° 
kon 2. Oe 
Re = Loe =>) 


where @, is a selected value for the servo bandwidth. 


For a rigid-body case, the stiffnesses of all springs approach infinity, 


and the 6, approaches zero. Equation (5.3) is reduced to a second-order 


ordinary differential equation : 


T, = Jieaica + Fiza (5.6) 
where 
Gin = An +ka(n - 4.) +ko(dn - 4.) (Sul) 
and the values of the feedback gains according to the ITAE are : 
, = 1.40, ow) 
ke 10a): (5-10) 


The fourth-order system requires four measurements for each link to 
compute the driving torques. It was assumed that the angular positions 
and velocities of both links and motors are available from shaft encoders 
and tachometers. The second and third derivatives of link positions (1,e., 
accelerations and jerks) are not measurable. Equation (5.4) shows that 
the position, velocity, acceleration and jerk of link are required to compute 
the torques to drive the plant. An estimator in a recursive form 1s 
designed to estimate those variables that are not measurable (1., q, and q, 
) by using the measurements from those variables that are accessible (Le., 
iid; and q,,,). 

By substituting Equation (4.20) into Equation (4.17), it becomes 

n, =1,0, +Q,(1,@,)+R;"n,,, +S, xf, +Cl, x F sky 


oy 


then substituting Equation (4.24) into Equation (5.11) gives 
n, =LRi1(Q,,@,,+ @,, + ®,,)+Q,(Io,)+Ri"n,,, 
+S, xf, +Cl, xF (5.12) 
Substituting Equation (5.12) into Equation (4.8) yields 
k,5, ={Ri_[LR"(Q,,0,, + @,, + @,,) +2,(1,0,) 
+Ri'n,,, +S, xf, +Cl, xF]}-k (5.13) 
Equation (5.13) 1s rearranged as 
(Ri_LRi@,,)-k =k,6,-{Ri[LR"(Q,,0,, + @,,)+2,(1,o,) 
-Ri'n,,, + SI, xf, +Cl, xF]}-k (5.14) 
Recall Equation (4.28) where 


S 
II 
oO 


it 


O: 


therefore Equation (5.14) can be rewritten as 


q; = IR) (k.5, = {Ri_[LRO (Q,_,@),; + @,_,] + Q, (1,a; } 
Vii ]33 


-Ri*'n,,, + Sl, xf, +Cl, x F,]}-k (5.15) 


represents the element of the third column and the 


where (Rj_I,R>").,, 
third row in matrix (Ri_,L.R*") . 
Again, Differentiating Equation (4.8) gives 
k,6, =(Ri_,Q,n, +Ri_,n,)-k +(Ri_n,)-Q,,k (5.16) 


Similarly, referring to Equations (4.17), (4.18), (4.21), (4.25)and (4.28), 


Equation (5.16) becomes 
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eee ] oo | 
q; = (RoR), (hs —{Ri_Qun, + Ri_,[20,1,0, + Q.Q1,0, 


i-lG a3 


+Q.1,0, > Q:N; + IRF (Oe. O,, + Q2, O,., + 202; Q,, 


+Q,., QO), + O,, a Q.(SI, x f; ) + SI, = f, a Q (CI, a F) 


Ri_n,)-Qk (4.48) 


+CL xF]}-k)-epRsy | ii 


pene: 
where 
6, =G.4,-4,, 

For the rigid-body controller case, as 6, approaches zero, the 
number of degree of freedom of link i reduces to one. The system 
governing equation then becomes a second-order ordinary differential 
equation. Since the required feedbacks q, and q, are measurable , no 


estimation is needed. 
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VI. SUMMARY 


A. CONCLUSIONS 

A direct dynamic model and an inverse dynamic model have been 
developed for multi-link robot manipulators with joint flexibility. Since an 
additional degree of freedom was considered for joint flexibility, two sets 
of system dynamic equations were derived by the Lagrangian method for 
direct dynamics which returns the robot joint motions according to input 
torques. The first equation described the large motions of links while the 
second described the small motions of twist angles of joint springs. These 
two equations are expressed in a form suited for the sequential integration 
method [Ref. 8]. Moreover, the Lagrangian method is systematic and 
easy to program. 

The Newton-Euler method was used to develop the inverse dynamics 
which returns the required torques assuming the robot joint motions are 
known. The Newton-Euler method provides efficient computation and 
has potential for the on-line control purpose. The inverse dynamics for 
multi-link robot manipulators was derived by decoupling the link dynamics 
and motor dynamics. A fourth-order system equation of link variables 
was obtained and a fourth-order error dynamics was designed for 
controller. This control was finally achieved by the design of a recursive 
estimator which estimates the second and third time derivatives of link 
positions. The advantage of the estimator is to eliminate the differentiation 


process that would cause noise problems. 
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B. RECOMMENDATIONS 
The recommendations are listed as follows: 
1. Continue to perform computer simulation for multi-link robot 
manipulators with joint flexibility. 
2. Evaluate the performance of flexible-body controller. 


3. Implement the flexible-body controller for on-line control. 
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APPENDIX A. DERIVIATION OF THE RESIDUAL TERM 


The residual acceleration mentioned in this thesis includes all the 
nonlinear terms caused by the Coriolis acceleration and centripetal 
acceleration due to link motion. The residual accelerations can be 
computed through a recursive form and computational efficiency can be 
therefore improved since the detailed computational procedure of each 
nonlinear term is skipped. 

Recall Equation (3.26): 

W. = dea as Da Gh (Aam) 
k=] m=1 k=1 

The residual acceleration is defined as 


W. = > Winn i (A.2) 


k=1 m=1 


The recursive form W, is derived as follows. The derivation begins 


with the recursive relation of homogeneous transformation matrix W,, 
W, and W.. 


W.=W_A (A.3) 
Therefore, 
W, = W._A, + WA, (A.4) 
W, = W_,A, +2W,,A, + W,_A, (A.5) 
since 
A. =QA4, (A.6) 
A, = QA.q, + QA.4G, (At 
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where 


00 0 0 
00 -1 0 

QFlo 1 0 0 oo 
00 0 0 


is called the operative matrix for revolute joints. For prismatic joints, it 


4 


W. =W_A,+W,A, + W_,QA.q, + W_QA.q, + W_QA,G, 


becomes 


— oS SS 
SoS o> 
S 2 ©& 

=> =, 2 © 


Oe) 


The recursive form of W., is rewritten as 


(A.10) 
If i=1, 
W =W,A, + WA, + W,QA,d, + W,QA,4, + W,QA,4, (A.11) 
where 
1000 
v I @ © ale 
ipo oe 
10 0 0 1 
then 
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W, =W, =0 (A123 
and (A.11) becomes 
W, = W,QA.4, + W,QA,4, 


= W,QQA,q,q, + W,QA,q, (A.14) 
According to Equation (A.1), Equation (A.14) can be expressed as 
W, = W,, + W,QA.G, (A.15) 
where 
W,, = W,QQA,q,q, (A.16) 
If 1=2, 


W, = W,A, + WA, + WQA, 4d, + W,QA,G, + WQA,G, 
= WA, + W,QA,d, + W,QA,q, + W,QQA, 4,4, + WQA,G, 
= WA, +2W,QA,q, + W,QQA,4,g, +WQA,g,  (A.17) 
Substituting equation (A.15) into (A.17) yields 


W, =(W,, + W,QA,q, JA, +2W,QA,q, + W,QQA,4,4, + W,QA,4, 


=(W,A, +2W,QA,q, + W,QQA,4,4,) + W,QA,A04, 
+W,QA,q, (A.18) 
where 
W., = WA, + 2W,QA,q, + W,QQA,¢,4, (A.19) 
Similarly, the rest of the residual terms can be found in a recursive 
manner as 1=3,4,5,6 and so on. Therefore, a general form of residual 
terms can be obtained as 
W,, = W,,,A, +2W,_,QA,q, + W,,QQA,4.4, (A.20) 
where ?=l2 2... 6 
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APPENDIX B. DERIVATION OF DIRECT DYNAMICS 


A direct dynamics is modeled by using the Lagrangian method. The 


Lagrangian equation is given as 





| ACRE) |_ 9B ALPE) pong 
a De , aé a aE ey ¢ | i ae n) GH 


where ¢ are the generalized coordinates, n is the number of degrees of 
freedom and P. denotes the generalized forces. 


The generalized coordinates are defined as 
fk 
S = [ayceedno On... OF 


where the generalized coordinate 4; for large motion represents the 
angular position of link i and the generalized coordinate 6, for small 
motion represents the angular displacement between 4; (1.e., link position 


variable) and motor position variable 4,,. 


The generalized force P. obtained from the concept of virtual work is 


defined as 
OW - Dail oo > T.(G,6 q;- o) 0; ) 


where TG, 1s the generalized force acting on the generalized coordinate 
q,, and —T. is the generalized force acting on the generalized coordinate 
6. 


Two dynamic equations, named the dynamic equations of large motion 


and the dynamic equations of small motion according to generalized 


oF 


coordinates, are derived separately in general forms for the sequential 


integration method in simulation. 


A. THE DYNAMIC EQUATION OF LARGE MOTION 
The dynamic equation of large motion is derived from Lagrangian 


equation with generalized coordinate of link. Performing the differentiation 


of Equation (3.7) with respect to q ,obtains 


O(KE) x 1S, OW. OW, 
aq “soe ise oa (SE | (B.1) 





by introducing the notation 








OW. OW 
—= -=W., for ;>j (Be 
dq, oq, : 
Equation (B.1) can be rewritten as 
GUE) ee las 7 <s . 
———- =—striw JW +WJwW.. B.3 
where 
tr( W,,J,W, ) = tr(W,J, W,, ) (B.4) 
and (B.3) becomes 
A(KE) x 
tr| W, J,W, Ba 
04, = ( ‘) (B.5) 
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by substituting Equation (B.4) into Equation (B.3). 


Differentiate Equation (B.5) with respect to time gives 


(KE), — 
4) 04, x De (W, ,J;W, +W,J,W, ) (B.6) 


since 


W,, = DW red, (B.7) 


Equation (B.6) becomes 


ONIRE) (5 | - N] 
oe dq, “|. Eu] 2 Wind il eA (B.8) 


izj 
The second term of the Lagrange equation due to link kinetic energy 


is obtained by differentiating Equation (3.7) with respect to q;: 


Cie, || o Weer ee | 0) WV. 
cfs bom JW, wa [5 ) 


J J 


6 (OW _.., 
Sof Sn (B.9) 


See eG) 





where 





and 


W. = yw Gh (B.10) 


ay 


OW, .. 
aa ee =>W ae for paapeen (B.11) 
q, 


Equation (B.9) then becomes 


sre aCe a 


J 


Similarly, performing the differentiation of Equation (3.14) with respect 





to q, obtains 
6 0 W. O A ; ae | 
ar = ur ———A_J_A‘, W.. + W,, =I ALW,, 
d qj ist 2 0 q; @ 4; 








aA.) aw.) 
+W._A_J., -) W.4+W_A J At 
' er | ‘ ) 


dW. a es ae 
+2} ——A_ JAW! +W,_,——J_A UW, 
Oq, aay 


. Tt ye 
OA... _— . (coe 
teal J : Me a Wea) AC ° 
0 q, d q, 


d NY. ‘ ck T 0 A, cack iT 
i ° Ad mins WV, a5 We ° JA, 
dq 0 


i J 














mi” mi O q 


y] J 


, 1p 
yee aw 
SF ie A J ~ We + WA mi Ae So (Bails) 
q 


Since A_, are not functions of q,, and i > j +1, thus 


d Aw 


=0 
a4), 
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+ |=0 
04d, 
By introducing the notation 


OW, 


——+=W., (B.14) 
dq, : 
Equation (B.13) becomes 
K 6 i 
ARE a = ¥ deel(W.AwJwALW,, +W,,A,J,,ALWS,) 
O q; i=j+l 2 





J 


| Pyne 
Wa AabAALW Wi Aata| : = w| 








OM = , aA). 
aa ——_ J Aceve tA YS W._, |7(B.15) 
i dq, mi mi i i mi mé O t 


as 
Gr Waren AL WV 7 (WA, J An Wo, (B .16) 


and 








OA tae Se 
tr ee : ee VY, = i alg a ° vee (B.17) 
d q; 0 q; 


one can finally obtain 


4] 


ORE). = Yufw,, ASniAm Wir +Wh Ani) Aw Wea | 


0 q. s=j+l lacie 


7 
6 | aA_\)_, 2) ..— 
+>) tr My Ae . Ww ar WwW, . = J ie 
i=l a, q. ‘a 0 q, 


/ J 











(B.18) 
Introducing the notations as follows: 
| _ 9 Aw =A... where /=/ (B.19) 
mi i Ge Gy mJ 
dn = G4; - 6, (B.20) 
6 = _ Aw) i= j 
21] a |= De hee 
substituting Equation (B.21) into Equation (B.18), Equation (B.18) 
becomes 
KE 6 A: 
Ae SW, An wAuW., HW, Aga An Wo] 
q; i= j+] 


+tr[W,,A,J,,G,A,,,W) +W,G,A,,J,,A,, W,,, |B.22) 
Differentiating Equation (B.22) with respect to time obtains 

d| AKE), | u(W,_.A,J,,G,A,,W), +W,A,J,G,A,,W. 

at dq, J yom Js wy, j-} J : j) 


+W,A,J,,G,A,,W +W,A,J,,GA, WwW" 


my my 


+W GA Jat mW. Ges AG We 


m,j° my mj my 


+W,.G,A,, J,,A,,W,., +W,4,J,,G,A,,,W 
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6 e T ° 
+ Ytr(W, AnJ An W,, +W, AJ An We, 
i=j+l 
+W,_, A.J ;A,;W,, + W,, A.J A; W,, 
+W,_,A,J,,A,;W,, + W,, A.J /A,;W,, 


+W, Ag Ans Woy + Wea Ad Acs We | (B.23) 


Tm 


where 


ir(W, A J,,G,A_,,W.,)=t(W,A,J,G,A,,W) | 


mj*~ mj 


Equation (B.23) then becomes 


Ml AKE) =t(W,,A,J,,G,A_,,W) +2W,A,J,,G,A,,,W_ 
at Od, J my mj” mj 


+W. A_ JAW, Dae Ae A JA W., 


t-l,jo  mi~ mi i-l,j° omi~ mi 


+W GA ee + W,4.G,A A AR 


mj,j~ my mj,j~ my 


+W,, An niAns WW, | 


F y BW, AuedigAe, WV. +W,, Asid nin WV. 


i=j+1 


+2 W. A J A W. SAP AAE A JA .W., 


i-l,j° omi~ mi i-l,/ 
+W,_, A.J ,;A,,W,, + W,A,J A.W, 
+W,, An Jn Aw WY, | (B.24) 


The second term of Equation (3.1) due to motor kinetic energy 1s 





0 (KE 6 OW. OA. . 
2 (KE), n - = ———A_ JA‘. Ww, +W., ~J AW, 
dq, | 40 dq a 


J J 


T . Ti 
O Aa eas sale NA 
+W_A_ J. W.,+W_A_ JA, 
Oq, oq, 


Bi J 
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OW.., nO A 
+2) ——A_J_,A.,W,, + W., J An Wi, 
d qd; 0 q; 


| AS oe ..(9W,,) 
BN née soygll ie 5 W,.. + WAL A 








oq, 


° 


J 








aWa. oe OA, . 
‘| od 7 (me nese + Wi = MVE Ne 











q; 0 qj 
_ (dA,)—. . faw,) 
BN cle ty WwW, + Wj Ae 7. (B.25) 
on oq, 
OW. aW | — 
As the terms 5 and 5 exist only when i2j+1 the terms 





dA. dA, - 
and exist only when j > j, and 
oq, j 
° ° T 
aw. a | _(aw,, 
tr ee ae =U Viele we yO 
d q; 0 q; 








° I A. T war : ) 13, : x77 
ir Wes, 3 J a = tp Miele all W.., 
q; 0 qj 


T 
d Wei; AT \x7T A T d Ww sted, 
tr AS ee = tr Ve ae 
O q; ) q; 








j 


: ‘ T 
d A i A T rc ‘ 0 fe a 
tr W.., “ J Aen = tr Wied ape 
0 Oq, 


Equation (B.25) can be rewritten as 





0 (KE é o W. . oW 
ERE). FE => ay 2———A_J_A™W!, +2 —A_J_AW,, 
8q, am Aq, 0q, 





J 
T 
ype) OW, 
2, ASL) SM | aie = Agee An W,., 
Oq, Od. 


X 








| OA. , Al- -. 
tulw,, SSUES NAR Derm 7; Cae 


2 Oq., my my qe 


m/ iT 


Z 
. T 
: a 0 a 
ea ee VY ac Vy Lees A 
J y ony Oq, J Hf Oq. y y 





ni 





(B.26) 
Introducing the notation 
a a (B.27) 
Oq, uy 
Equation (B.26) becomes 
“* de (WA JAW, + W, An Jan W,, 
+W, A.J An Wo, + We Aad mm WV, } 
+tr( WGA.) JyAny Wea + WieiG Ag Jy Amy W 
+W,_,A,J,,G,A,,,W +W,.G,A,,J,,A,W) (B.28) 
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The last term of Equation (3.1) is obtained by differentiating Equation 
(3.20) 


d (PE) é Tet 
——=-)imr' W,g-m_r/ A, W.g- ym A, W,_, g (B.29) 
‘al q; i=j t=j+] ‘ 
Introducing the notations 
X, = W,_A,, (B.30) 
X,=W.A,+W_A, (B.31) 
X,=W.,A,+2W_A_ WA (B.32) 


and combining Equations (B.8), (B.12), (B.24), (B.28) and (B.29) 
according to Equation (3.1) gives the final general form of dynamic 
equation of large motion, 1.e., 


Sr(W,,J,W, ) +tr(XJ,,A5,,W)+ + Yu(XJ, AGW oe 


t=j+] 


-Y mr! Wg m,,r/,A,, W.g- mr A 


t=j+l 


i W,. 8 =G,T, 


(Bia8) 


B. THE DYNAMIC EQUATION OF SMALL MOTION 
The dynamic equation of small motion is derived from Lagrangian 
equation with generalized coordinate of deflection. The first term in 


Equation(3.1) can be written as 


d (9(KE) )_d(0(KE),) d {0 (KE), 
a\ as, | a\ a6, | al a6 


where 
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d(9(KE)x )_, 
dt| 06. i 


therefore, performing the differentiation of Equation (3.14) with respect to 


5, gives 





0 (KE 6 OW, . OA, 
O (RE) a Smtr SA I a Vile — Je Ae Wee 
a6, 2 || a6 a6 


J J 





| Bee | aw.) 
+W.A_J — | Wi 4+W_A_ JA. .| —— 


oO W,, MORE 7 ' od ae , T aL 
cere Mea ate WV) aoe) WV, 
Ee 00. 


J J 


: oO Nee : T : ree i od Vale ; 
sta coer: : W.., =) Alen’ * \ are a 2 P) 5 








06 


j 


OW... JA, 
| EE AAW + We e JAG Win 





a6. 


J 


+W._A_J i P +W_A JA’ IW. B.34 
t-1 mi~ m 06, i-] i-] m* nm mi By ( : ) 


J 





i-] 


0 OW, .OA,, | | 
Peeiie terms ——— , ——and = are not functions of 6, thus 
05,’ 06, a6 : 





dW... 
= 
06. 


J 
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i-] = 0 
0 6, 
JA. 
—=0 
06 } 
and 
on a 
tr) W__ A.J ve we = tr} W., ——J_,A_,W.., 
a6 ; 006, 
d A. ° ° 
where exists only when i 2 j, Equation (B.34) then becomes 


a6 


i 


A (KE), 1 aA, : aA. \y. . 
— =" ==] 2W,,A_J,, bs +2W_,| —=|J_A 
‘al é, oh O 5 J J A 5, ji mj j 








(B.35) 


introducing the notation 





=e (B.36) 


Equation (B.35) becomes 


9 (KE), _ u(- 
06, 


Differentiating Equation (B.37) with respect to time obtains 


-W,A,JVA,,.W7-WA, JA, "W. *)(B.37) 


mj~ mj mj,j~ mj 
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d { a (KE). 
—| SA ir(-W AJA Wi -W AJA WwW" 


-W,,A,JA,.W,,-W,,A, JA, Wi, 


-W,,A J_,A,,W., -W,,A J_,A,,W., 


ny,j~ my mj,j~ ny 


-W,.A,, JAW, — WA 1 ALW) (B.38) 


mj,j~ mj mj, j~ my 


The second term of Equation (3.1) is obtained as 





dO (KE OA OA 
pike). bn 51 W,, Avy ATW! ~—+2W. —~J AW, 











Ob 0, JE 06, 5) 0, 
OA OA. .,.., 
+2W veN nals a EN ~jJ A_W., |\(B.39) 
pm / O 0, y i if 
and as 
J An A mae (B.40) 
a5, 88 ) 
iA )=-A -2 As (B.41) 
dt mj} mj.j AS 
Equation (B.39) can be rewritten as 
0 o (KE), 
——* SW Ae Nae Ny AL JA. W. 
j 


“WAS Am Weai-WadgJyAw Wa] (B.42) 


The last term of Equation (3.1) is obtained by differentiating Equation 
(3.20) with respect to 6: 


49 


d (PE) _ -. 
a6, =—m,Fa (~Ag,, )Wu8 +, 6, (B.43) 





Since 


= [2o | 0 


dt\ 06, 

0 
(KE). _ 
a6 

O (PE),, 

pan 
5 


combining Equations (B.38), (B.42) and (B.43) according to Equation 


(3.1) gives the final general form of dynamic equation of small motion, 1,e., 


-tr(X,J,,A,,,W,, )+m,rZ A, W,.g+k,6, =-T, (B.44) 
where 
X,=W,,A,, (B.45) 
xX, =W,_,A,,+W,_A,, (B.46) 
X, =W,,A,+2W,,A,W._A,. (B.47) 


50 


APPENDIX C. NEWTON-EULER RECURSIVE 
FORMULATION 


Referring back to the definition of the absolute angualr velocity of link 

i for revolute joint: 
wo, =Ri"(@,,+@,,) (C.1) 
By premultiplying rotation transformation matrices, the angular velocity of 


link i 1s transformed as 
Ria, =Ri'(@,,+@,,] (€2) 
Differentiating Equation (C.2) w.r.t. time according to R, = Rj, yields 
RiQ,@, +Ri@, =Ri'Q,_,(@,, + @,,)+R5"(@,, + @,,) (es) 
since 


Q.@-=0 


equation (C.3) becomes 
Rio, = RQ, ,(@,, + @,,)+ RS (@,, + @,, ] (C.4) 
multiplying Equation (C.4) by R° in both sides gives 
@, =Ri"(Q,,@,,+ @,, + @,,) (C5) 
Where 


0 
RiR =R‘" and R°R’ = 0 (C.6) 


os 2 
SS =—_ © 


Similarly, differentiating Equation (C.6) once and twice w.r.t time gives 


5) 


, a ie 2... (Oz W) ; a QO, a5 Q: 0, + 202, QO, Bs Om 25 QO, a Qa; 


(Ca) 
, = R> (ohm Q, ; tp Deo ie QO. + AD ysl De Oj + 3Q; ,Q:@,, 

+20, ,@,, +3Q,,6,,+Q,,Q,.,0,, +3Q,,0,,+9,,0,, 

+6. + ,, ~ (22,0, +22, 6,) (C.8) 


The linear acceleration of link i expressed in terms of base coordinate 
is given as 
Ria, = Ri (Ria, +Q,0,S1, +Q,S1, (C.9) 
differentiating Equation (C.9) w.r.t. time yields 


RiQ,a, + Ria, = RF (Q,,a,, +4,,) + Ri (Q,Q,Q, +20,Q, + QQ, +Q,)SI, 


i-1" t= 1 


(Calg) 
By multiplying R}in both sides, Equation (C.10) can be rewritten in a 


recursive form of derivative of the linear acceleration, 1.e., 

a, = Ri" (Q,,a,, +4,,) +(Q? +2N,Q, +N,Q, +, )S1,-Q,a,  (C.11) 
Similarly, differentiating Equation (C.11) gives the second derivative of 
linear acceleration, 1.e., 

4, = Ri1(Q?,a,, +2Q,,a,, +Q,,a,, +4), ) 
+ (QF + 3Q7Q, +Q,Q,0, +20,0,Q, + 3Q} + 3Q,Q, +9,Q, +O,)SI, 
—(Q?a, + Qa, + 20.a, | (C1 
The derivatives of force and torque can be obtained by following the 


same way. The results are as follows: 
f=F+R"f., (C135 


DV 


f,=Q.F +6 +R" (Q,,f,, +£.,)-Qf, 


i+] i+] i+] 


i 


i+] 


f =Q.Q.F +207 +O,F +F +R (Q,,Q.0f,, +2Q,,, 
-(Q,O.f, + 20, + Q.f,) 
F=m,a, 
F =m, 
vet OC. Sal HOH Gb 
a, =a, + Q,Cl, + Q,Q,Cl, + Q,Q,Cl, 
4, = 4, +0,Cl, +Q.Q,Cl, +20,0,Cl, +Q,0.C1, 
N,=L@, +Q,1.q, 
N, =1,0, +Q,1L@, +Q,1,a, 
N, = 1,0, + Q,],@, +2Q,1,@, + Q1,0, 
n =N,+R"n,,, +S, xf, +Cl, x F 


oe? 


i+] i+] 


(C.14) 
+f... ] 
(CxE5) 


(C.16) 
(C.17) 
(C.18) 
(C.19) 
(C.20) 
(C.21) 
(eo?) 
(C.23) 
(E24) 
(E25) 


n, =Q,N, +N, +Ri"(Q,,,n,,, +,,,) +Q,(SI, x f,) +S], xf, +Q,(Cl, x F) 


+Cl, x F -Qn, 


i+] itl 


(C.26) 
fi, =Q,.Q,N, +2Q,N,+0O.N,4+N, +R (Q,,Q,,.n,,, +2Q,,,0, 


i+] i+] 


+Q..n,,, +1i,,,)+Q,Q,(SI, xf,) +2, (SI, xf.) +2, (SI, xf.) 


+$1, x f, +2,Q,(Cl, x F,) +2Q,(Cl, x F,)+2,(Cl, x F,) + Cl, x F, 


-Q,.0.n, - 20.8, - Qn, 


3 


(e.27 ) 
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